#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np
import serial
import time

class OrangeDetector(Node):
    def __init__(self):
        super().__init__('orange_detector')
        
        # 定义橙色HSV范围（可根据实际环境调整）
        self.lower_orange = np.array([15, 102, 184])
        self.upper_orange = np.array([30, 255, 255])
        
        # 创建OpenCV桥接器
        self.bridge = CvBridge()
        
        # 订阅图像话题（根据实际摄像头节点修改）
        self.image_sub = self.create_subscription(
            Image,
            'image_raw',  # USB摄像头发布的话题名
            self.image_callback,
            10)
        self.image_sub  # 防止未使用变量警告
        
        # 串口配置（根据实际端口修改）
        try:
            self.ser = serial.Serial(
                port='/dev/ttyUSB0',  # Linux系统串口（Windows：COM7）
                baudrate=115200,
                bytesize=serial.EIGHTBITS,
                parity=serial.PARITY_NONE,
                stopbits=serial.STOPBITS_ONE,
                timeout=0.1
            )
            self.get_logger().info(f"串口已打开: {self.ser.port}")
        except serial.SerialException as e:
            self.get_logger().error(f"串口打开失败: {e}")
            self.ser = None
        
        # 帧结构常量（与STM32协议匹配）
        self.FRAME_HEADER = 0x66
        self.FRAME_LENGTH = 0x07
        self.FRAME_TAIL = 0x5B
        self.SEND_INTERVAL = 0.05  # 发送间隔（20Hz）
        
        # 发送时间控制
        self.last_send_time = time.time()
        
        self.get_logger().info("橙色目标检测节点已启动")

    def send_coordinate(self, x, y):
        """按STM32协议发送坐标数据（二进制帧）"""
        if not self.ser or not self.ser.is_open:
            return

        # 将坐标转换为16位无符号整数
        x_clamped = np.clip(x, 0, 65535)
        y_clamped = np.clip(y, 0, 65535)

        # 拆分X、Y为高低字节
        x_low = x_clamped & 0xFF
        x_high = (x_clamped >> 8) & 0xFF
        y_low = y_clamped & 0xFF
        y_high = (y_clamped >> 8) & 0xFF

        # 构建完整帧
        frame = bytearray([
            self.FRAME_HEADER,
            self.FRAME_LENGTH,
            x_low,
            x_high,
            y_low,
            y_high,
            self.FRAME_TAIL
        ])

        # 发送帧数据
        try:
            self.ser.write(frame)
            # 调试信息（可选）
            self.get_logger().debug(f"发送帧: {[hex(b) for b in frame]}  坐标: ({x_clamped},{y_clamped})")
        except serial.SerialException as e:
            self.get_logger().error(f"发送失败: {e}")

    def image_callback(self, msg):
        """处理接收到的图像消息"""
        try:
            # 将ROS图像消息转换为OpenCV格式
            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        except Exception as e:
            self.get_logger().error(f"图像转换失败: {e}")
            return

        # 图像预处理
        hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
        mask = cv2.inRange(hsv, self.lower_orange, self.upper_orange)
        kernel = np.ones((5, 5), np.uint8)
        mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)

        # 查找目标轮廓
        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        target_x, target_y = 0, 0  # 默认为(0,0)
        detected = False

        # 筛选最大轮廓
        if contours:
            max_contour = max(contours, key=cv2.contourArea)
            area = cv2.contourArea(max_contour)
            if area > 100:  # 过滤小面积噪点
                # 计算目标中心坐标
                M = cv2.moments(max_contour)
                if M["m00"] > 0:
                    target_x = int(M["m10"] / M["m00"])
                    target_y = int(M["m01"] / M["m00"])
                    detected = True
                    # 在图像上标记目标
                    cv2.circle(cv_image, (target_x, target_y), 5, (0, 255, 0), -1)
                    cv2.putText(cv_image, f"({target_x},{target_y})",
                                (target_x + 10, target_y),
                                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)

        # 按间隔发送数据
        current_time = time.time()
        if current_time - self.last_send_time > self.SEND_INTERVAL:
            self.send_coordinate(target_x, target_y)
            self.last_send_time = current_time

        # 显示图像（调试用）
        cv2.imshow("Target Detection", cv_image)
        cv2.waitKey(1)

def main(args=None):
    rclpy.init(args=args)
    orange_detector = OrangeDetector()
    try:
        rclpy.spin(orange_detector)
    except KeyboardInterrupt:
        pass
    finally:
        # 释放资源
        cv2.destroyAllWindows()
        if orange_detector.ser and orange_detector.ser.is_open:
            orange_detector.ser.close()
            orange_detector.get_logger().info("串口已关闭")
        orange_detector.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
